/*
 * state_machine_test.cpp
 *
 *  Created on: Jan 18, 2013
 *      Author: michiel
 */

// Bring in gtest
#include <gtest/gtest.h>
#include <rosbag/bag.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <ar_pose/ARMarkers.h>

#include "PS3Controller/PS3_buttons.h"
#include "rp_states/state_machine.h"

ros::Publisher joy_pub_;
ros::Publisher marker_pub_;

TEST(StateMachine, initialization) {
	StateMachine machine;
	EXPECT_EQ(machine.getState(), IDLE);
}

TEST(StateMachine, idleToDrive) {
	StateMachine machine;
	machine.setState(MOVING_WITHOUT_BALL);
	machine.stateSwitcher();
	EXPECT_EQ(machine.getState(), MOVING_WITHOUT_BALL);
}

int main(int argc, char **argv) {
	ros::init(argc, argv, "State Machine Tester");
	// Create NodeHandle.
	ros::NodeHandle nh_;

	// Subscribe to topics.
	joy_pub_		= nh_.advertise<sensor_msgs::Joy>(Topic::PS3_JOY, 1);
	marker_pub_		= nh_.advertise<ar_pose::ARMarkers>(Topic::AR_POSE, 1);

	testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}



